﻿using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
using System;


namespace cubeAdjustment
{
    internal class RobustEstimatesFunc
    {
        /// <summary>
        /// 最小二乘法权
        /// </summary>
        /// <param name="Residual"></param>
        /// <returns></returns>
        public static Matrix<double> Square(Vector<double> Residual)
        {
            var n = Residual.Count;
            var P = new DenseMatrix(n);
            for (int i = 0; i < n; i++)
            {
                    P[i, i] = 1;
            }
            return P;
        }

        /// <summary>
        /// Huber法选权迭代算子
        /// </summary>
        /// <param name="Residual">需要转换的向量</param>
        /// <param name="c">常系数，可以取值为2σ</param>
        /// <returns>权矩阵</returns>
        public static Matrix<double> Huber(Vector<double> Residual, double c)
        {
            var n = Residual.Count;
            var P = new DenseMatrix(n);
            for (int i = 0; i < n; i++)
            {
                if (Math.Abs(Residual[i]) <= c)
                {
                    P[i, i] = 1;
                }
                else
                {
                    P[i, i] = c / Math.Abs(Residual[i]);
                }
            }
            return P;
        }

        /// <summary>
        /// Hample选权迭代算子
        /// </summary>
        /// <param name="Residual"></param>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="c"></param>
        /// <returns></returns>
        public static Matrix<double> Hampel(Vector<double> Residual, double sigma)
        {
            var a = 0.5 * sigma;
            var b = 1.5 * sigma;
            var c = 2.5 * sigma;
            var n = Residual.Count;
            var P = new DenseMatrix(n);
            for (int i = 0; i < n; i++)
            {
                if (Math.Abs(Residual[i]) <= a)
                {
                    P[i, i] = 1;
                }
                else if (Math.Abs(Residual[i]) > a && Math.Abs(Residual[i]) <= b)
                {
                    P[i, i] = a / Math.Abs(Residual[i]);
                }
                else if (Math.Abs(Residual[i]) > b && Math.Abs(Residual[i]) <= c)
                {
                    P[i, i] = a * (c - Math.Abs(Residual[i])) / ((c - b) * Math.Abs(Residual[i]));
                }
                else
                {
                    P[i, i] = 0;
                }
            }
            return P;
        }

        /// <summary>
        /// IGG法选权迭代算子
        /// </summary>
        /// <param name="Residual"></param>
        /// <param name="sigma"></param>
        /// <param name="k"></param>
        /// <returns></returns>
        public static Matrix<double> IGG(Vector<double> Residual, double sigma)
        {
            var n = Residual.Count;
            var P = new DenseMatrix(n);
            double k = Residual.Sum() / Residual.Count * 1e-3;
            for (int i = 0; i < n; i++)
            {
                if (Math.Abs(Residual[i]) <= 1.5 * sigma)
                {
                    P[i, i] = 1;
                }
                else if (Math.Abs(Residual[i]) > 1.5 * sigma && Math.Abs(Residual[i]) <= 2.5 * sigma)
                {
                    P[i, i] = 1 / (Math.Abs(Residual[i]) + k);
                }
                else
                {
                    P[i, i] = 0;
                }
            }
            return P;
        }

        /// <summary>
        /// Tukey法选权迭代算子
        /// </summary>
        /// <param name="Residual"></param>
        /// <param name="k"></param>
        /// <returns></returns>
        public static Matrix<double> Tukey(Vector<double> Residual, double k)
        {
            var n = Residual.Count;
            var P = new DenseMatrix(n);
            for (int i = 0; i < n; i++)
            {
                if (Math.Abs(Residual[i]) <= k)
                {
                    P[i, i] = 1;
                }
                else if (Math.Abs(Residual[i]) > k && Math.Abs(Residual[i]) <= 2 * k)
                {
                    P[i, i] = 1 - Math.Pow((Math.Abs(Residual[i]) - k) / k, 2);
                }
                else
                {
                    P[i, i] = 0;
                }
            }
            return P;
        }

        /// <summary>
        /// 一次范数最小法选权迭代算子
        /// </summary>
        /// <param name="Residual"></param>
        /// <returns>权矩阵</returns>
        public static Matrix<double> L1(Vector<double> Residual)
        {
            var k = Residual.Sum() / Residual.Count * 1e-3;
            var n = Residual.Count;
            var P = new DenseMatrix(n);
            for (int i = 0; i < n; i++)
            {
                P[i, i] = 1 / (k + Math.Abs(Residual[i]));
            }
            return P;
        }

        /// <summary>
        /// p范法选权迭代算子
        /// </summary>
        /// <param name="Residual"></param>
        /// <param name="k"></param>
        /// <param name="p"></param>
        /// <returns></returns>
        public static Matrix<double> Lp(Vector<double> Residual, double p)
        {
            var k = Residual.Sum() / Residual.Count * 1e-3;
            var n = Residual.Count;
            var P = new DenseMatrix(n);
            for (int i = 0; i < n; i++)
            {
                P[i, i] = 1 / (k + Math.Pow(Math.Abs(Residual[i]), 2 - p));
            }
            return P;
        }

        /// <summary>
        /// 丹麦法选权迭代算子
        /// </summary>
        /// <param name="Residual"></param>
        /// <param name="sigma"></param>
        /// <param name="k"></param>
        /// <param name="a"></param>
        /// <returns></returns>
        public static Matrix<double> Denmark(Vector<double> Residual, double sigma, double k, double a)
        {
            var n = Residual.Count;
            var P = new DenseMatrix(n);
            for (int i = 0; i < n; i++)
            {
                if (Math.Abs(Residual[i]) <= 2 * sigma)
                {
                    P[i, i] = 1;
                }
                else
                {
                    P[i, i] = a * Math.Exp(-k * Math.Pow(Residual[i], 2));
                }
            }
            return P;
        }
    }
}